#include <zephyr.h>
#include <sys/printk.h>
#include <shell/shell.h>
#include <shell/shell_uart.h>
#include <logging/log.h>

#include <version.h>
#include <stdlib.h>
#include <gpio.h>
#include <board.h>
#include <pwm.h>
#include <led.h>
#include "nrf52.h"
#include "nrf.h"
#include "test.h"

LOG_MODULE_REGISTER(app);

static void swo_cfg_enable()
{
	CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
	*((volatile unsigned *)(ITM_BASE + 0x400F0)) = 0x00000002;										   /* "Selected PIN Protocol Register": Select which protocol to use for trace output (2: SWO NRZ, 1: SWO Manchester encoding) */
	*((volatile unsigned *)(ITM_BASE + 0x40010)) = 499;												   /* "Async Clock Prescaler Register". Scale the baud rate of the asynchronous output */
	*((volatile unsigned *)(ITM_BASE + 0x00FB0)) = 0xC5ACCE55;										   /* ITM Lock Access Register, C5ACCE55 enables more write access to Control Register 0xE00 :: 0xFFC */
	ITM->TCR = ITM_TCR_TraceBusID_Msk | ITM_TCR_SWOENA_Msk | ITM_TCR_SYNCENA_Msk | ITM_TCR_ITMENA_Msk; /* ITM Trace Control Register */
	ITM->TPR = ITM_TPR_PRIVMASK_Msk;																   /* ITM Trace Privilege Register */
	ITM->TER = 1;																					   /* ITM Trace Enable Register. Enabled tracing on stimulus ports. One bit per stimulus port. */
	*((volatile unsigned *)(ITM_BASE + 0x01000)) = 0x400003FE;										   /* DWT_CTRL */
	*((volatile unsigned *)(ITM_BASE + 0x40304)) = 0x00000100;										   /* Formatter and Flush Control Register */

	NRF_CLOCK->TRACECONFIG = (CLOCK_TRACECONFIG_TRACEMUX_Serial << CLOCK_TRACECONFIG_TRACEMUX_Pos) | 0x3;
	NRF_P0->PIN_CNF[18] = (GPIO_PIN_CNF_DRIVE_H0H1 << GPIO_PIN_CNF_DRIVE_Pos) | (GPIO_PIN_CNF_INPUT_Connect << GPIO_PIN_CNF_INPUT_Pos) | (GPIO_PIN_CNF_DIR_Output << GPIO_PIN_CNF_DIR_Pos);
}

static int (*__printk_out)(int);

static int semi_console_out(int character)
{
	if(__printk_out)
		__printk_out(character);
	return (int)ITM_SendChar((uint32_t)character);
}

static void cmd_semihost(const struct shell *shell, size_t argc, char **argv)
{
	swo_cfg_enable();
	__printk_out = __printk_get_hook();
	__printk_hook_install(semi_console_out);
	__stdout_hook_install(semi_console_out);
}

static void cmd_version(const struct shell *shell, size_t argc, char **argv)
{
	ARG_UNUSED(argc);
	ARG_UNUSED(argv);

	printk("Zephyr version %s\n", KERNEL_VERSION_STRING);
}

static void cmd_clk(const struct shell *shell, size_t argc, char **argv)
{
	ARG_UNUSED(argc);
	ARG_UNUSED(argv);

	printk("Zephyr CLK %llu\n", (u64_t)sys_clock_hw_cycles_per_sec);
	printk("Zephyr trick %u\n", k_cycle_get_32());
	k_sleep(100);
	printk("Zephyr trick now %u\n", k_cycle_get_32());
}

static void cmd_demo_ping(const struct shell *shell, size_t argc, char **argv)
{
	ARG_UNUSED(argc);
	ARG_UNUSED(argv);

	shell_fprintf(shell, SHELL_NORMAL, "pong\r\n");
}

static void cmd_demo_params(const struct shell *shell, size_t argc, char **argv)
{
	int cnt;

	shell_fprintf(shell, SHELL_NORMAL, "argc = %d\r\n", argc);
	for (cnt = 0; cnt < argc; cnt++) {
		shell_fprintf(shell, SHELL_NORMAL,
				"  argv[%d] = %s\r\n", cnt, argv[cnt]);
	}
}

SHELL_CREATE_STATIC_SUBCMD_SET(sub_demo)
{
	/* Alphabetically sorted. */
	SHELL_CMD(params, NULL, "Print params command.", cmd_demo_params),
	SHELL_CMD(ping, NULL, "Ping command.", cmd_demo_ping),
	SHELL_SUBCMD_SET_END /* Array terminated. */
};

SHELL_CMD_REGISTER(demo, &sub_demo, "Demo commands", NULL);
SHELL_CMD_REGISTER(ver, NULL,"Show kernel version", cmd_version);
SHELL_CMD_REGISTER(clk, NULL, "get clk", cmd_clk);
SHELL_CMD_REGISTER(semi, NULL, "switch semi",  cmd_semihost);


void main(void)
{
	//net_test_init();
	bt_test_init();
}
